#pragma once

#include <cmath>

#include "hdmap/math/VectorBase.hpp"
#include "hdmap/math/VolumeBase.hpp"

#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif

namespace Navinfo
{
	namespace Engine
	{
		namespace Math
		{
			//constants
			const double HDMap_PI = 4.0 * atan(1.0);

			///convert from radians to degrees
			template< typename T >
			T Degrees(const T& rad)
			{
				T mul(180.0 / HDMap_PI);
				return rad * mul;
			}

			///convert from radians to degrees
			template< typename T >
			T Radians(const T& deg)
			{
				T mul(HDMap_PI / 180.0);
				return deg * mul;
			}

			///Linear interpolation
			template< typename T, typename U >
			T Lerp(const T& a, const T& b, U factor)
			{
				return T(a + factor * (b - a));
			}

			///Clamp a value between min and max
			template< class T, class U >
			T Clamp(const T& in, const U& min, const U& max)
			{
				if (in < min)
				{
					return min;
				}
				else if (in > max)
				{
					return max;
				}

				return in;
			}

			///Get the minimum value of a pair
			template< typename T >
			T Min(const T& in1, const T& in2)
			{
				return in1 < in2 ? in1 : in2;
			}

			///Get the maximum value of a pair
			template< typename T >
			T Max(const T& in1, const T& in2)
			{
				return in1 > in2 ? in1 : in2;
			}

			///linear distance between 2 scalars
			template< typename T >
			T Distance(const T& lhs, const T& rhs)
			{
				return T(std::sqrt(lhs * rhs));
			}

			///linear distance between 2 vectors
			template< typename T, unsigned int N >
			T Distance(const VectorBase< T, N >& lhs, const VectorBase< T, N >& rhs)
			{
				T temp(0);
				for (unsigned int i = 0; i < N; ++i)
				{
					temp += lhs[i] * rhs[i];
				}

				return T(std::sqrt(temp));
			}

			///calculates the spherical distance between 2 angles (in radians)
			///multiply the result by the radius of the sphere in the prefered unitsystem
			template< typename T >
			T SphericalDist(const VectorBase< T, 2 >& coord1, const VectorBase< T, 2 >& coord2)
			{
				return std::acos(std::sin(coord1.y_) * std::sin(coord2.y_) +
					std::cos(coord1.y_) * std::cos(coord2.y_) *
					std::cos(coord2.x_ - coord1.x_));
			}

			///calculates the spherical distance between 2 angles (in radians)
			///multiply the result by the radius of the sphere in the prefered unitsystem
			template< typename T >
			T SphericalDist(const VectorBase< T, 3 >& coord1, const VectorBase< T, 3 >& coord2)
			{
				return std::acos(std::sin(coord1.y_) * std::sin(coord2.y_) +
					std::cos(coord1.y_) * std::cos(coord2.y_) *
					std::cos(coord2.x_ - coord1.x_));
			}


			///returns component wise min
			template< typename T, unsigned int N >
			VectorBase< T, N > Min(const VectorBase< T, N >& lhs, const VectorBase< T, N >& rhs)
			{
				VectorBase< T, N > temp;
				for (unsigned int i = 0; i < N; ++i)
				{
					temp[i] = lhs[i] < rhs[i] ? lhs[i] : rhs[i];
				}
				return temp;
			}

			///Returns component wise max
			template< typename T, unsigned int N >
			VectorBase< T, N > Max(const VectorBase< T, N >& lhs, const VectorBase< T, N >& rhs)
			{
				VectorBase< T, N > temp;
				for (unsigned int i = 0; i < N; ++i)
				{
					temp[i] = lhs[i] > rhs[i] ? lhs[i] : rhs[i];
				}
				return temp;
			}

			///dot product of 2 vectors
			template< typename T, unsigned int N >
			T Dot(const VectorBase< T, N >& lhs, const VectorBase< T, N >& rhs)
			{
				T tmp(0);
				for (unsigned int i = 0; i < N; ++i)
				{
					tmp += lhs[i] * rhs[i];
				}
				return tmp;
			}

			///Length of the vector
			template< typename T, unsigned int N >
			T Length(const VectorBase< T, N >& vec)
			{
				T sqrLen = Dot(vec, vec);
				return T(std::sqrt(sqrLen));
			}

			///Normalize vector
			template< typename T, unsigned int N >
			VectorBase< T, N > Normalize(const VectorBase< T, N >& vec)
			{
				T len(Length(vec));
				if (len != 0)
				{
					return VectorBase< T, N >(vec / len);
				}
				else
				{
					return VectorBase< T, N >(0);
				}
			}
		} //namespace Math
	}
}
